#include "vertexcluster.h"

#include <windows.h>
#include <sstream>

#include "DiagMatrix.h"
#include "consoleoutput.h"

#include "DeformableMesh.h"


const float VertexCluster::EPSLON = 0.0001f;
const float VertexCluster::C_YIELD = 0.5;
const float VertexCluster::C_CREEP = 1.0f;
const float VertexCluster::C_MAX = 1.0f;
const float VertexCluster::MIN_A_DET = 0.1f;
const float VertexCluster::MAX_A_DET = 1.3f;

VertexCluster::VertexCluster( DeformableMesh& mesh, int clusterId ) :
	mesh( mesh ),
	numberOfVertices( 0 ),
	clusterId( clusterId ),
	matR(),
	matA(),
	Aqq(),
	Sp( 3, 3 ),
	detA( 1.0f ),
	deepness( 0.0f ),
	connections()
{
	Sp( 0, 0 ) = 1.0f;
	Sp( 1, 1 ) = 1.0f;
	Sp( 2, 2 ) = 1.0f;
}

void VertexCluster::addVertex( 
	ModelOBJ::Vertex* vertex, 
	ModelOBJ::Vertex* speed, 
	ModelOBJ::Vertex* intermediatePosition, 
	ModelOBJ::Vertex* intermediateSpeed, float weight )
{
	m_vertexBuffer.push_back( vertex );
	vertexSpeed.push_back( speed );
	this->intermediatePosition.push_back( intermediatePosition );
	this->intermediateSpeed.push_back( intermediateSpeed );
	
	this->weight.push_back( weight );
	originalWeight.push_back( weight );

	originalVertexBuffer.push_back( m_vertexBuffer[ numberOfVertices ]->position[0] );
	originalVertexBuffer.push_back( m_vertexBuffer[ numberOfVertices ]->position[1] );
	originalVertexBuffer.push_back( m_vertexBuffer[ numberOfVertices ]->position[2] );
	numberOfVertices++;
}

void VertexCluster::initialize()
{
	//// storing original vertex buffer
	//for( int i = 0; i < numberOfVertices; i++ )
	//{
	//	originalVertexBuffer.push_back( m_vertexBuffer[ i ]->position[0] );
	//	originalVertexBuffer.push_back( m_vertexBuffer[ i ]->position[1] );
	//	originalVertexBuffer.push_back( m_vertexBuffer[ i ]->position[2] );
	//}

	calculateCenterOfMass( cm0, originalVertexBuffer, weight );
	cm[0] = cm0[0];
	cm[1] = cm0[1];
	cm[2] = cm0[2];

	// initializing p vectors
	p.clear();
	q.clear();
	for( int i = 0; i < numberOfVertices * 3; i++ )
	{
		p.push_back( 0.0f );
		q.push_back( 0.0f );
	}

	// storing qi
	for( int i = 0; i < numberOfVertices; i++ )
	{
		q[ i* 3 + 0 ] = originalVertexBuffer[ i*3 + 0 ] - cm0[ 0 ];
		q[ i* 3 + 1 ] = originalVertexBuffer[ i*3 + 1 ] - cm0[ 1 ];
		q[ i* 3 + 2 ] = originalVertexBuffer[ i*3 + 2 ] - cm0[ 2 ];
	}

	// preloading
	preLoad();

	// loading and pre-calculating
	load();
}

void VertexCluster::load()
{
	// pre-calculating Aqq
	float matAqq[16];
	float matAqqInv[16];

	outerProduct( q.data(), q.data(), matAqq );

	Aqq.LoadMatrix( matAqq );
	Aqq.GetMatrixIT( matAqqInv );
	Aqq.LoadMatrix( matAqqInv );
}

float VertexCluster::update( float dt )
{
	deepness = 0.0f;

	calculateCenterOfMass( cm, intermediatePosition, weight );

	//CONSOLE( "Center of mass: (" << cm[ 0 ] << ", " << cm[1] << ", " << cm[2] << ")" );

	// calculating p (current relative positions of the mesh vertices)
	for( int i = 0; i < numberOfVertices; i++ )
	{
		p[ i * 3 + 0 ] = intermediatePosition[ i ]->position[0] - cm[0];
		p[ i * 3 + 1 ] = intermediatePosition[ i ]->position[1] - cm[1];
		p[ i * 3 + 2 ] = intermediatePosition[ i ]->position[2] - cm[2];
	}

	if( calculateRMatrix() )
	{
		calculateAMatrix();
		updateInternal( dt );

		//// calculating S matrix plasticity
		if( mesh.getPlasticityMode() != PLASTICITY_MODE_NONE )
			applyPlasticDeformation( dt );
	}

	return deepness;
}

void VertexCluster::outerProduct( const float* v1, const float* v2, float* mat )
{
	for( int i = 0; i < 15; i++ )
	{
		mat[ i ] = 0.0f;
	}
	mat[ 15 ] = 1.0f;

	// calculating pi*qi^T matrix
	for( int i = 0; i < numberOfVertices; i++ )
	{
		for( int j = 0; j < 9; j++ )
		{
			int l = j % 3;
			int c = (j / 3);

			int matIndex = j + ( j / 3 );

			// adding up
			mat[ matIndex ] +=  weight[ i ] * v1[ i * 3 + l ] * v2[ i * 3 + c ];
		}
	}
}

void VertexCluster::outerProduct( const float* v1, int size1, const float* v2, int size2, LAMatrix& mat, float weight )
{
	for( int i = 0; i < size1; i++ )
		for( int j = 0; j < size2; j++ )
			mat( i, j ) += v1[ i ] * v2[ j ] * weight;
}

bool VertexCluster::calculateRMatrix()
{
	outerProduct( p.data(), q.data(), rawApq );

	DiagMatrix Apq;
	Apq.LoadMatrix( rawApq );

	float rawApqT[16];
	Apq.GetMatrixT( rawApqT );

	Apq.LoadMatrix( rawApqT );
	Apq.MultMatrix( rawApq );

	float ApqTApq[16];

	// getting the square of the matrix
	Apq.SquareRoot();

	// inverting the matrix
	Apq.GetMatrix( ApqTApq );
	DiagMatrix ApqTApqSquared;
	ApqTApqSquared.LoadMatrix( ApqTApq );	

	// since the matrix is symmetric,
	// we are only inverting it!
	float rawApqTApqSquared[16];
	ApqTApqSquared.GetMatrixIT( rawApqTApqSquared );

	// calculating R rotation matrix
	VMatrix newMatR;
	newMatR.LoadMatrix( rawApq );
	newMatR.MultMatrix( rawApqTApqSquared );

	// checking if new R matrix is a rotation matrix
	float newMatRDet = newMatR.Determinant();
	//CONSOLE( "Det R: " << newMatRDet );

	if( abs( newMatRDet - 1.0f ) < EPSLON )
	{
		float rawNewMatR[ 16 ];
		newMatR.GetMatrix( rawNewMatR );
		matR.LoadMatrix( rawNewMatR );
	//	return true;
	}
	//else
	//{
		//float rawNewMatR[ 16 ];
		//newMatR.GetMatrix( rawNewMatR );
		//matR.LoadMatrix( rawNewMatR );
	//}
	return true;
}

void VertexCluster::applyPlasticDeformation( float dt )
{
	//if( recalculateWeights )
	//	onWeightChanged();

	// calculating S matrix
	float rawRT[ 16 ];
	float rawA[ 16 ];
	matR.GetMatrixT( rawRT );
	matA.GetMatrix( rawA );

	LAMatrix S( 3, 3 );
	LAMatrix A( 3, 3 );
	for( int i = 0; i < 3; i++ )
	{
		for( int j = 0; j < 3; j++ )
		{
			S( i, j ) = rawRT[ i + j * 4 ];
			A( i, j ) = rawA[ i + j * 4 ];
		}
	}
	S *= A;

	// calculating norm
	S( 0, 0 ) -= 1.0f;
	S( 1, 1 ) -= 1.0f;
	S( 2, 2 ) -= 1.0f;

	float norm = S.norm();

	/*ConsoleOutput::SSTR << "||S - I|| = " << norm;
	ConsoleOutput::Print();*/
	if( norm > C_YIELD )
	{
		if( mesh.getPlasticityMode() == PLASTICITY_MODE_UPDATE )
		{
			// we apply our plastic deformation
			LAMatrix cCreepMatrix( 3, 3 );

			for (int i = 0; i < 3; i++)
				for (int j = 0; j < 3; j++)
					cCreepMatrix( i, j ) = S( i, j );

			cCreepMatrix *= C_CREEP;
			//cCreepMatrix.Print();

			//VMatrix newSp;
			LAMatrix newSp( 3, 3 );
			newSp( 0, 0 ) = 1.0f;
			newSp( 1, 1 ) = 1.0f;
			newSp( 2, 2 ) = 1.0f;
			newSp += cCreepMatrix;
			newSp *= Sp;
			//newSp.print();

			for (int i = 0; i < 3; i++)
				for (int j = 0; j < 3; j++)
					Sp( i, j ) = newSp( i, j );


			newSp( 0, 0 ) -= 1.0f;
			newSp( 1, 1 ) -= 1.0f;
			newSp( 2, 2 ) -= 1.0f;

			float newSpNorm = newSp.norm();

			// checking if our deformation exceeds cMax
			if( newSpNorm > C_MAX )
			{
				// too much!
				newSp *= C_MAX / newSpNorm;
			}
			newSp( 0, 0 ) += 1.0f;
			newSp( 1, 1 ) += 1.0f;
			newSp( 2, 2 ) += 1.0f;

			for (int i = 0; i < 3; i++)
				for (int j = 0; j < 3; j++)
					Sp( i, j ) = newSp( i, j );

			// normalizing the Sp matrix, to preserve volumes
			float sPDet = Sp.det();
			sPDet = ( sPDet < 0? -1.0f: 1.0f ) * pow( sPDet < 0? -sPDet: sPDet, 1.0f/3.0f );
			Sp *= 1.0f / sPDet;

			/*ConsoleOutput::SSTR << "1/det = " << 1.0f / sPDet;
			ConsoleOutput::Print();*/
		}
		//else
		//{
		//	// updating original buffer
		//	for( int i = 0; i < numberOfVertices; i++ )
		//	{
		//		originalVertexBuffer[ i * 3 + 0 ] = p[ i * 3 + 0 ];
		//		originalVertexBuffer[ i * 3 + 1 ] = p[ i * 3 + 1 ];
		//		originalVertexBuffer[ i * 3 + 2 ] = p[ i * 3 + 2 ];
		//	}

		//	// reloading center of mass
		//	calculateCenterOfMass( cm0, originalVertexBuffer, weight );
		//}

		updateQVector( Sp );
		load();
		//onWeightChanged();
		//load();
	}
}

void VertexCluster::calculateAMatrix()
{
	matA.LoadMatrix( rawApq );

	float rawAqq[16];
	Aqq.GetMatrix( rawAqq );
	matA.MultMatrix( rawAqq );

	// dividing the A matrix by its determinant so we preserve volumes
	detA = matA.Determinant();
	detA = max( detA, MIN_A_DET );
	detA = min( detA, MAX_A_DET );
	//CONSOLE( "Det A: " << detA );

	float detACubed = ( detA < 0? -1.0f: 1.0f ) * pow( detA < 0? -detA: detA, 1.0f/3.0f );
	matA.ScalarMult3x3( 1.0f/detACubed );
}

void VertexCluster::preLoad()
{
}

void VertexCluster::calculateCenterOfMass( float* cm, std::vector<float>& pos, std::vector<float>& weight )
{
	cm[0] = cm[1] = cm[2] = 0.0f;
	float totalMass = getTotalMass();

	//CONSOLE( "Total mass: " << totalMass );

	for( int i = 0; i < numberOfVertices; i++ )
	{
		cm[ 0 ] += pos[ i*3 + 0 ] * weight[ i ];
		cm[ 1 ] += pos[ i*3 + 1 ] * weight[ i ];
		cm[ 2 ] += pos[ i*3 + 2 ] * weight[ i ];
	}
	cm[ 0 ] /= totalMass;
	cm[ 1 ] /= totalMass;
	cm[ 2 ] /= totalMass;
}

void VertexCluster::calculateCenterOfMass( float* cm, std::vector<ModelOBJ::Vertex*>& pos, std::vector<float>& weight )
{
	cm[0] = cm[1] = cm[2] = 0.0f;
	float totalMass = getTotalMass();

	for( int i = 0; i < numberOfVertices; i++ )
	{
		cm[ 0 ] += pos[ i ]->position[0] * weight[ i ];
		cm[ 1 ] += pos[ i ]->position[1] * weight[ i ];
		cm[ 2 ] += pos[ i ]->position[2] * weight[ i ];
	}
	cm[ 0 ] /= totalMass;
	cm[ 1 ] /= totalMass;
	cm[ 2 ] /= totalMass;
}

void VertexCluster::calculateRotationAngles( VMatrix& matR, float* rotX, float* rotY, float* rotZ )
{
	float xy = matR.Get( 2, 1 );
	float xx = matR.Get( 1, 1 );
	float xz = matR.Get( 3, 1 );
	float yz = matR.Get( 3, 2 );
	float zz = matR.Get( 3, 3 );
	*rotZ = atan2(xy, xx);
	*rotY = -asin(xz);
	*rotX = atan2(yz,zz);
}

float VertexCluster::getMeshBeta()
{
	return mesh.getBeta();
}

float VertexCluster::getMeshAlpha()
{
	return mesh.getAlpha();
}

//void VertexCluster::calculatingIntermediateSpeedAndPosition( float dt )
//{
//}

//float VertexCluster::applyCollisionConstraints( float dt )
//{
//	//int vertexUnderTheGroundCount = 0;
//	for (int i = 0; i < numberOfVertices; i++)
//	{
//		if( intermediatePosition[ i ].position[ 1 ] < -1.5f )
//		{
//			//vertexUnderTheGroundCount++;
//			intermediateSpeed[ i ].position[ 0 ] *= RESTITUTION;
//			intermediateSpeed[ i ].position[ 1 ] = -( intermediatePosition[ i ].position[ 1 ] + 1.5f ) / dt;
//			intermediateSpeed[ i ].position[ 2 ] *= RESTITUTION;
//
//			intermediatePosition[ i ].position[ 1 ] += dt * intermediateSpeed[ i ].position[ 1 ];
//		}
//	}
//
//	//float interpenetration = ( ( float ) vertexUnderTheGroundCount * 100 ) / ( ( float ) numberOfVertices );
//	return 0.0f;
//}

void VertexCluster::updateSpeedAndPosition( const float* relativeGi, int vertexIndex, float dt )
{
	const float meshAlpha = getMeshAlpha();
	float gPos[3];
	gPos[ 0 ] = relativeGi[ 0 ] + cm[ 0 ];
	gPos[ 1 ] = relativeGi[ 1 ] + cm[ 1 ];
	gPos[ 2 ] = relativeGi[ 2 ] + cm[ 2 ];

	//// updating speed
	vertexSpeed[ vertexIndex ]->position[ 0 ] += weight[ vertexIndex ] * meshAlpha * ( ( gPos[ 0 ] - intermediatePosition[ vertexIndex ]->position[ 0 ] ) / dt );
	vertexSpeed[ vertexIndex ]->position[ 1 ] += weight[ vertexIndex ] * meshAlpha * ( ( gPos[ 1 ] - intermediatePosition[ vertexIndex ]->position[ 1 ] ) / dt );
	vertexSpeed[ vertexIndex ]->position[ 2 ] += weight[ vertexIndex ] * meshAlpha * ( ( gPos[ 2 ] - intermediatePosition[ vertexIndex ]->position[ 2 ] ) / dt );

	m_vertexBuffer[ vertexIndex ]->position[ 0 ] += weight[ vertexIndex ] * ( ( gPos[ 0 ] * meshAlpha ) + ( 1.0f - meshAlpha ) * intermediatePosition[ vertexIndex ]->position[ 0 ] );
	m_vertexBuffer[ vertexIndex ]->position[ 1 ] += weight[ vertexIndex ] * ( ( gPos[ 1 ] * meshAlpha ) + ( 1.0f - meshAlpha ) * intermediatePosition[ vertexIndex ]->position[ 1 ] );
	m_vertexBuffer[ vertexIndex ]->position[ 2 ] += weight[ vertexIndex ] * ( ( gPos[ 2 ] * meshAlpha ) + ( 1.0f - meshAlpha ) * intermediatePosition[ vertexIndex ]->position[ 2 ] );

	//// updating speed
	/*intermediateSpeed[ vertexIndex ]->position[ 0 ] += weight[ vertexIndex ] * meshAlpha * ( ( gPos[ 0 ] - intermediatePosition[ vertexIndex ]->position[ 0 ] ) / dt );
	intermediateSpeed[ vertexIndex ]->position[ 1 ] += weight[ vertexIndex ] * meshAlpha * ( ( gPos[ 1 ] - intermediatePosition[ vertexIndex ]->position[ 1 ] ) / dt );
	intermediateSpeed[ vertexIndex ]->position[ 2 ] += weight[ vertexIndex ] * meshAlpha * ( ( gPos[ 2 ] - intermediatePosition[ vertexIndex ]->position[ 2 ] ) / dt );

	intermediatePosition[ vertexIndex ]->position[ 0 ] += weight[ vertexIndex ] * ( ( gPos[ 0 ] * meshAlpha ) + ( 1.0f - meshAlpha ) * intermediatePosition[ vertexIndex ]->position[ 0 ] );
	intermediatePosition[ vertexIndex ]->position[ 1 ] += weight[ vertexIndex ] * ( ( gPos[ 1 ] * meshAlpha ) + ( 1.0f - meshAlpha ) * intermediatePosition[ vertexIndex ]->position[ 1 ] );
	intermediatePosition[ vertexIndex ]->position[ 2 ] += weight[ vertexIndex ] * ( ( gPos[ 2 ] * meshAlpha ) + ( 1.0f - meshAlpha ) * intermediatePosition[ vertexIndex ]->position[ 2 ] );*/

	updateDeepness( m_vertexBuffer[ vertexIndex ]->position[ 1 ] );
}

void VertexCluster::setVertexWeight( int index, float weight )
{
	this->weight[ index ] = weight; 
}

float VertexCluster::getVertexWeight( int index )
{
	return this->weight[ index ];
}

float VertexCluster::getTotalMass()
{
	float totalMass = 0.0f;
	for( int i = 0; i < numberOfVertices; i++ )
		totalMass += weight[ i ];
	return totalMass;
}

void VertexCluster::onWeightChanged()
{
	calculateCenterOfMass( cm0, originalVertexBuffer, weight );
	cm[0] = cm0[0];
	cm[1] = cm0[1];
	cm[2] = cm0[2];

	q.clear();
	for( int i = 0; i < numberOfVertices * 3; i++ )
	{
		q.push_back( 0.0f );
	}

	// storing qi
	for( int i = 0; i < numberOfVertices; i++ )
	{
		q[ i* 3 + 0 ] = originalVertexBuffer[ i*3 + 0 ] - cm0[ 0 ];
		q[ i* 3 + 1 ] = originalVertexBuffer[ i*3 + 1 ] - cm0[ 1 ];
		q[ i* 3 + 2 ] = originalVertexBuffer[ i*3 + 2 ] - cm0[ 2 ];
	}

	// preloading
	preLoad();

	// loading and pre-calculating
	load();
}

void VertexCluster::resetWeights()
{
	//for( int i = 0; i < numberOfVertices; i++ )
	//	weight[ i ] = originalWeight[ i ];
}

void VertexCluster::updateDeepness( float yPosition )
{
	deepness = max( deepness, -(yPosition - DeformableMesh::FLOOR_HEIGHT ) );
}

void VertexCluster::recalculateCenterOfMass()
{
	calculateCenterOfMass( cm, intermediatePosition, weight );
}
